#include "mapping/navigation_planner.h"

void Navigation_planner::DEBUG_pub_grid_map(){
    grid_mapROS.header.frame_id = map_info.frame_id;
    grid_mapROS.info.resolution = map_info.res;
    grid_mapROS.header.stamp = ros::Time::now();
    unsigned int all_data_length = map_info.map_height * map_info.map_width;
    grid_mapROS.info.height = map_info.map_height;
    grid_mapROS.info.width = map_info.map_width;
    grid_mapROS.info.origin.position.x = map_info.map_ori_x;
    grid_mapROS.info.origin.position.y = map_info.map_ori_y;
    grid_mapROS.info.origin.position.z = map_info.map_ori_z;

    grid_mapROS.data.resize(all_data_length);
    for(unsigned int i = 0;i < all_data_length;i++){
        // int current_data = int(grid_map[i]);
        // if(current_data == 255){
        //     current_data = -1;
        // } 
        if(grid_map[i] == 255){ // former -1 node -> 255 unsigned int 
            grid_mapROS.data[i] = -1;
            //LOG(INFO) << "val = 255";
            continue;    
        }
        grid_mapROS.data[i] = grid_map[i];
    }
    
        ros::Duration(3).sleep();
        pub_grid_map.publish(grid_mapROS);
}
void Navigation_planner::clicked_point_callback(const geometry_msgs::PointStampedConstPtr& msg_in){
    mtx.lock();

    Eigen::Vector3d point_in(msg_in->point.x,msg_in->point.y,msg_in->point.z);
    destinations.push_back(point_in);
    LOG(INFO) << "clicked point recieved. x = " << point_in.x() << " y = " << point_in.y() << " z = " << point_in.z();
    for(unsigned int i = 0;i < destinations.size();i++){
        LOG(INFO) << "point " << i << " = " << destinations[i].transpose();
    }

    mtx.unlock();
}
bool Navigation_planner::server_plan_with_gps_callback(mapping::plan_with_gpsRequest& req,mapping::plan_with_gpsResponse& res){
    mtx.lock();
    if(!transform_destinations_to_LLA()){
        res.success = false;
        mtx.unlock();
        return true;
    }
    res.success = true;
    mtx.unlock();
    return true;
}
bool Navigation_planner::service_clear_destinations_callback(mapping::clear_destinationsRequest& req,mapping::clear_destinationsResponse& res){
    mtx.lock();

    destinations.clear();
    LOG(INFO) << "destinations points cleared.";

    res.success = true;
    mtx.unlock();
    return true;
}
bool Navigation_planner::server_show_path_callback(mapping::show_pathRequest& req,mapping::show_pathResponse& res){
    nav_msgs::Path path;
    path.header.stamp = ros::Time::now();
    path.header.frame_id = "map";
    for(int i = 0;i < destinations.size();i++){
        geometry_msgs::PoseStamped pose_i;
        pose_i.pose.position.x = destinations[i].x();
        pose_i.pose.position.y = destinations[i].y();
        pose_i.pose.position.z = destinations[i].z();
        path.poses.push_back(pose_i);
    }
    pub_path.publish(path);
    res.success = true;
    return true;
}
void Navigation_planner::save_lla_to_txt(){
    if(destinations_lla.empty()){
        LOG(WARNING) << "destinations_lla is empty.call service plan_with_gps first.";
        return ;
    }
    std::ofstream of_lla;
    std::string of_lla_path = std::string(PROJECT_PATH) + "data/detinations_lla.txt";
    of_lla.open(of_lla_path,std::ios::out);
    if(!of_lla.is_open()){
        LOG(ERROR) << "error opening file " << of_lla_path << " when saving destinations_lla.";
        return;
    }
    for(int i = 0;i < destinations_lla.size();i++){
        of_lla << std::fixed << std::setprecision(8) <<destinations_lla[i].latitude << " " << destinations_lla[i].longitude << " " << destinations_lla[i].altitude << std::endl;
    }
    LOG(INFO) << "destinations_lla save into " << of_lla_path << " sucessfully.";
}
void Navigation_planner::DEBUG_print_destinations_lla(){
    LOG(INFO) << "destinations in map size = " << destinations.size();
    LOG(INFO) << "destinations in LLA size = " << destinations_lla.size();
    for(int i = 0;i < destinations_lla.size();i++){
        LOG(INFO) << std::fixed << std::setprecision(8)  << "destinations_lla[" << i << "] = " 
                  << destinations_lla[i].latitude << " " << destinations_lla[i].longitude << " " << destinations_lla[i].altitude;
    }
}
bool Navigation_planner::transform_destinations_to_LLA(){
    if(destinations.empty()){
        LOG(ERROR) << "destinations is empty,clicked point in RVIZ,and then retry.";
        return false;
    }
    if(!geo_converter_inited){
        LOG(INFO) << "geo_converter_inited = " << geo_converter_inited;
        LOG(ERROR) << "geo_converter not inited,correct plan_with_gps in yaml.";
        return false;
    }
    //destinations_lla.resize(destinations.size());
    destinations_lla.clear();
    double des_lat,des_lon,des_alt,des_x,des_y,des_z;
    LLA des_lla;
    for(int i = 0;i < destinations.size();i++){
        des_x = destinations[i].x();
        des_y = destinations[i].y();
        des_z = destinations[i].z();

        geo_converter.Reverse(des_x,des_y,des_z,des_lat,des_lon,des_alt);
        des_lla.latitude  = des_lat;
        des_lla.longitude = des_lon;
        des_lla.altitude  = des_alt;
        destinations_lla.push_back(des_lla);
    }
    CHECK(destinations.size() == destinations_lla.size()) << 
         "destinations_LLA.size() != destinations.size() after transformation in  Navigation_planner::transform_destinations_to_LLA";
    save_lla_to_txt();
    DEBUG_print_destinations_lla();
}
Navigation_planner::Navigation_planner(ros::NodeHandle& nh_){
    std::string yaml_path = std::string(PROJECT_PATH) + "/config/navigation_planner.yaml";
    YAML::Node node = YAML::LoadFile(yaml_path);

    map_data_path = node["map_data_path"].as<std::string>();
    map_yaml_path = node["map_yaml_path"].as<std::string>();
    plan_with_gps = node["plan_with_gps"].as<bool>();
    //在需要进行轨迹跟随时  需要保证 plan_with_gps = true
    if(plan_with_gps){
        std::string origin_gps_path = node["ori_gps_path"].as<std::string>();
        YAML::Node ori_gps_node = YAML::LoadFile(origin_gps_path);

        double ori_lat,ori_lon,ori_alt;
        ori_lat = ori_gps_node["latitude"].as<double>();
        ori_lon = ori_gps_node["longitude"].as<double>();
        ori_alt = ori_gps_node["altitude"].as<double>();
        
        LOG(INFO) << "plan_with_gps : true";

        origin_gps.altitude  = ori_alt;
        origin_gps.longitude = ori_lon;
        origin_gps.latitude  = ori_lat;

        geo_converter.Reset(ori_lat,ori_lon,ori_alt);
        geo_converter_inited = true;

        LOG(INFO) << std::fixed << std::setprecision(8) << "ori_lat = " << origin_gps.latitude;
        LOG(INFO) << std::fixed << std::setprecision(8) << "ori_lon = " << origin_gps.longitude;
        LOG(INFO) << std::fixed << std::setprecision(8) << "ori_alt = " << origin_gps.altitude;
    }
    map_loader = std::make_shared<Map_loader>(map_yaml_path,map_data_path);

    nh = nh_;

    pub_grid_map              = nh.advertise<nav_msgs::OccupancyGrid>("grid_map",100);
    pub_path                  = nh.advertise<nav_msgs::Path>("path",1000);
    sub_clicked_point         = nh.subscribe<geometry_msgs::PointStamped>("/clicked_point",1000,&Navigation_planner::clicked_point_callback,this);
    server_clear_destinations = nh.advertiseService("navigation_planner/clear_detinations",&Navigation_planner::service_clear_destinations_callback,this); 
    server_show_path          = nh.advertiseService("navigation_planner/show_path",&Navigation_planner::server_show_path_callback,this);
    server_plan_with_gps      = nh.advertiseService("navigation_planner/plan_with_gps",&Navigation_planner::server_plan_with_gps_callback,this);

    map_loader->get_map_data(&grid_map);
    map_loader->get_map_info(map_info);

    destinations.clear();

    DEBUG_pub_grid_map();
}










